{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# PreintegrationCombinedParams\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/PreintegrationCombinedParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "## Overview\n",
        "\n",
        "The `PreintegrationCombinedParams` class holds parameters specifically required for the `CombinedImuFactor` and its associated `PreintegratedCombinedMeasurements` class. \n",
        "\n",
        "It inherits from `PreintegrationParams` (and thus also `PreintegratedRotationParams`) and adds parameters that model the *evolution* of the IMU bias over time, typically as a random walk process. This is essential for the `CombinedImuFactor`, which estimates biases at both the start ($b_i$) and end ($b_j$) of the preintegration interval."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install gtsam-develop if not installed\n",
        "try:\n",
        "    import gtsam\n",
        "except ImportError:\n",
        "    %pip install gtsam-develop"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Key Parameters\n",
        "\n",
        "In addition to all parameters from `PreintegrationParams`, this class adds:\n",
        "\n",
        "- **`biasAccCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the accelerometer bias. Units: (m²/s⁵)/Hz ? (Check docs, represents variance growth rate).\n",
        "- **`biasOmegaCovariance`**: A 3x3 matrix representing the continuous-time covariance of the *random walk process* driving the gyroscope bias. Units: (rad²/s³)/Hz ? (Check docs, represents variance growth rate).\n",
        "\n",
        "**Important Note** (as of July 2025): There used to be a parameter `biasAccOmegaInit`, a 6x6 matrix representing the covariance of the uncertainty in the *initial* bias estimate provided to the preintegration. However, this should be done by adding a prior factor on the first bias."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Mathematical Background: Bias Random Walk\n",
        "\n",
        "The `CombinedImuFactor` implicitly assumes that the bias evolves between time steps according to a random walk:\n",
        "$$ b_{k+1} = b_k + w_k, \\quad w_k \\sim \\mathcal{N}(0, Q_b \\Delta t) $$ \n",
        "where $b_k = [b_{a,k}; b_{g,k}]$ is the 6D bias vector at time $k$, $w_k$ is zero-mean Gaussian noise, and $Q_b$ is the block-diagonal continuous-time covariance matrix formed from `biasAccCovariance` and `biasOmegaCovariance`:\n",
        "$$ Q_b = \\begin{bmatrix} \\text{biasAccCovariance} & 0 \\\\ 0 & \\text{biasOmegaCovariance} \\end{bmatrix} $$ \n",
        "The factor uses this model (integrated over the interval $\\Delta t_{ij}$) to constrain the difference between $b_i$ and $b_j$."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Key Functionality / API\n",
        "\n",
        "- **Constructors**: \n",
        "  - `PreintegrationCombinedParams(n_gravity)`: Main constructor.\n",
        "  - `MakeSharedD(g=9.81)` / `MakeSharedU(g=9.81)`: Convenience static methods for NED/ENU frames.\n",
        "- **Setters**: Methods like `setBiasAccCovariance`, `setBiasOmegaCovariance`, `setBiasAccOmegaInit`, plus all setters inherited from `PreintegrationParams`.\n",
        "- **Getters**: Corresponding getters for the combined parameters, plus all inherited getters.\n",
        "- **`print` / `equals`**: Standard methods."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Usage Example\n",
        "\n",
        "Create parameters, often starting from the base `PreintegrationParams` settings, and then set the bias evolution parameters."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Combined IMU Preintegration Parameters:\n",
            "\n",
            "gyroscopeCovariance:\n",
            "0.0001      0      0\n",
            "     0 0.0001      0\n",
            "     0      0 0.0001\n",
            "accelerometerCovariance:\n",
            "[\n",
            "0.01    0    0\n",
            "   0 0.01    0\n",
            "   0    0 0.01\n",
            "]\n",
            "integrationCovariance:\n",
            "[\n",
            "1e-08     0     0\n",
            "    0 1e-08     0\n",
            "    0     0 1e-08\n",
            "]\n",
            "n_gravity = (    0     0 -9.81)\n",
            "biasAccCovariance:\n",
            "[\n",
            "1e-06     0     0\n",
            "    0 1e-06     0\n",
            "    0     0 1e-06\n",
            "]\n",
            "biasOmegaCovariance:\n",
            "[\n",
            "1e-08     0     0\n",
            "    0 1e-08     0\n",
            "    0     0 1e-08\n",
            "]\n"
          ]
        }
      ],
      "source": [
        "from gtsam import PreintegrationCombinedParams\n",
        "import numpy as np\n",
        "\n",
        "# 1. Create parameters for an ENU navigation frame (Z-up)\n",
        "params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
        "\n",
        "# 2. Set standard noise parameters (accel, gyro, integration)\n",
        "accel_noise_sigma = 0.1\n",
        "gyro_noise_sigma = 0.01\n",
        "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
        "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
        "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
        "\n",
        "# 3. Set bias random walk noise parameters (example values)\n",
        "bias_acc_rw_sigma = 0.001 # m/s^2 / sqrt(s) -> Covariance = sigma^2\n",
        "bias_gyro_rw_sigma = 0.0001 # rad/s / sqrt(s) -> Covariance = sigma^2\n",
        "params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
        "params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
        "\n",
        "print(\"Combined IMU Preintegration Parameters:\")\n",
        "params.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "These combined parameters are then passed to `PreintegratedCombinedMeasurements`."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Source\n",
        "- [PreintegrationCombinedParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationCombinedParams.h)\n",
        "- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.cpp) (Contains the definition of this class)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}